package simulator.position;

import lejos.robotics.navigation.Move.MoveType;
import lejos.robotics.navigation.Pose;
import simulator.labyrinth.Wall;


public class Position {
	private Point point;
	private Direction orientation; // in degrees
	
	public Position() {
		this(20,20,0);
	}
	
	public Position clone() {
		return new Position(getX(), getY(), getOrientation());
	}
	
	public Position(double x, double y, double orientation) {
		point = new Point(x, y);
		this.orientation = new Direction(orientation);
	}
	
	public Position(Pose pos) {
		point = new Point(pos.getX(), pos.getY());
		this.orientation = new Direction(pos.getHeading());
	}
	
	public void move(double distance) {
		double x = point.getPreciseX() + distance * Math.cos(Math.toRadians(orientation.getOrientation()));
		double y = point.getPreciseY() + distance * Math.sin(Math.toRadians(orientation.getOrientation()));

		point = new Point(x,y);
	}
	
	public void turn(double degrees) {
		this.orientation.rotate(degrees);
	}
	
	public double getX() {
		return point.getPreciseX();
	}
	
	public double getY() {
		return point.getPreciseY();
	}
	
	public Point getPoint() {
		return point;
	}
	
	public double getOrientation() {
		return orientation.getOrientation();
	}
	
	public double[] getPosition() {
		double[] position = { getX(), getY(), getOrientation() };
		return position;
	}
	
	@Override
	public boolean equals(Object other) {
		if (other == null)
			return false;
		if (!(other instanceof Position))
			return false;
		
		Position position = (Position) other;
		return Double.compare(this.getX(), position.getX()) == 0
				&& Double.compare(this.getY(), position.getY()) == 0;
	}
	
	@Override
	public int hashCode(){
		return (int) (getX()*getY()+100*getX()+9*getY()); 
	}
	
	public double getDistanceTo(Wall wall) {
		Position position = this.clone();
		double distance=255;
		if (Double.compare(wall.getStartPosition().getX(),wall.getEndPosition().getX()) == 0) {
			distance = Math.abs((wall.getStartPosition().getX()-this.getX())/Math.cos(Math.toRadians(position.getOrientation())));
			position.move(distance);
			if (Double.compare(position.getX(), wall.getStartPosition().getX())==0 && 
					Double.compare(position.getY(), wall.getStartPosition().getY())>=0 &&
					Double.compare(position.getY(), wall.getEndPosition().getY())<=0  ){

				//	System.out.println("getdistanceToWall:"+ wall.getStartPosition().getPreciseX()+"/"+wall.getStartPosition().getPreciseY()+"///"+wall.getEndPosition().getPreciseX()+"/"+wall.getEndPosition().getPreciseY()+"#"+distance);
				return distance;
			}
		}
		else {
			distance = Math.abs((wall.getStartPosition().getY()-this.getY())/Math.sin(Math.toRadians(position.getOrientation())));
			position.move(distance);
			if (Double.compare(position.getY(), wall.getStartPosition().getY()) == 0 && 
					Double.compare(position.getX(), wall.getStartPosition().getX()) >= 0 &&
					Double.compare(position.getX(), wall.getEndPosition().getX()) <= 0)
				return distance;
		}

		return -1;
	}

	public void add(MoveType moveType, double dist) {
		switch (moveType) {
			case TRAVEL:
				move(dist);
				break;
			case ROTATE:
				turn(dist);
				break;
			case ARC:
			default:
				break;
		}
	}
}
